/*------------------------------------------------------
                app层can总线接收的应用函数
@brief functions:
.
.
.
.
@attention:

@auther:RM Cherno
--------------------------------------------------------*/
#include <main.h>
#include "can_receive.h"

//=====================================================================================================
static motor_measure_t motor_measure[7];

static CAN_TxHeaderTypeDef chassis_tx_message;
static CAN_TxHeaderTypeDef gimbal_tx_message;

static uint8_t chassis_can_send_data[8];
static uint8_t gimbal_can_send_data[8];
/*-----------------base functions
--------------------------------------------------------
@brief:
*/
//获得电机数据
#define get_motor_measure(ptr, data)                                   \
    {                                                                  \
        (ptr)->last_ecd = (ptr)->ecd;                                  \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);           \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);     \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
        (ptr)->temperate = (data)[6];                                  \
    }
/*-----------------app functions
--------------------------------------------------------
@brief:下面的电机控制函数为基础的电机电流控制，范围在(-16384,16384)
*/
//
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor1 >> 8;
    chassis_can_send_data[1] = motor1;
    chassis_can_send_data[2] = motor2 >> 8;
    chassis_can_send_data[3] = motor2;
    chassis_can_send_data[4] = motor3 >> 8;
    chassis_can_send_data[5] = motor3;
    chassis_can_send_data[6] = motor4 >> 8;
    chassis_can_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

//
void CAN_cmd_gimbal(int16_t yaw, int16_t pitch, int16_t shoot, int16_t rev)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_GIMBAL_ALL_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    gimbal_can_send_data[0] = (yaw >> 8);
    gimbal_can_send_data[1] = yaw;
    gimbal_can_send_data[2] = (pitch >> 8);
    gimbal_can_send_data[3] = pitch;
    gimbal_can_send_data[4] = (shoot >> 8);
    gimbal_can_send_data[5] = shoot;
    gimbal_can_send_data[6] = (rev >> 8);
    gimbal_can_send_data[7] = rev;
    HAL_CAN_AddTxMessage(&GIMBAL_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

//
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

    switch (rx_header.StdId)
    {
    case CAN_3508_M1_ID:
    case CAN_3508_M2_ID:
    case CAN_3508_M3_ID:
    case CAN_3508_M4_ID:
    case CAN_YAW_MOTOR_ID:
    case CAN_PIT_MOTOR_ID:
    case CAN_TRIGGER_MOTOR_ID:
    {
        static uint8_t i = 0;
        // get motor id
        i = rx_header.StdId - CAN_3508_M1_ID;
        get_motor_measure(&motor_measure[i], rx_data);
        break;
    }

    default:
    {
        break;
    }
    }
}

/* 发送ID为0x700，使得3508进入快速设置id模式 */
void CAN_cmd_chassis_reset_ID(void)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = 0x700;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = 0;
    chassis_can_send_data[1] = 0;
    chassis_can_send_data[2] = 0;
    chassis_can_send_data[3] = 0;
    chassis_can_send_data[4] = 0;
    chassis_can_send_data[5] = 0;
    chassis_can_send_data[6] = 0;
    chassis_can_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

//获得电机数据指针
/* 只读 返回yaw轴电机数据指针 */
const motor_measure_t *p_get_yaw_motor_point(void)
{
    return &motor_measure[4];
}
/* 只读 返回pitch轴电机数据指针 */
const motor_measure_t *p_get_pitch_motor_point(void)
{
    return &motor_measure[5];
}
/* 只读 返回拨弹电机数据指针 */
const motor_measure_t *p_get_trigger_motor_point(void)
{
    return &motor_measure[6];
}
/* 只读 返回底盘电机数据指针 用i进行索引*/
const motor_measure_t *p_get_chassis_motor_point(uint8_t i)
{
    return &motor_measure[(i & 0x03)];
}

//=====================================================================================================

/*-----------------XXX task functions
--------------------------------------------------------
@brief:
*/
//

/*-----------------XXX API_c
--------------------------------------------------------
@brief:
*/
//

/*------------------------end-------------------------*/